CON
GRAV = 280
pKp = 0.7
pKi = 1.0
pKd = 0

OBJ
'mathlib : "F32"
floatlib : "Float32Full"
k[2] : "KalmanFilter"

VAR
long accX, accY, accZ, gyroX, gyroY, gyroZ, pitch, roll, bearing
long stack[100], sumX[10], sumY[10]
long cog, int1, int2
byte quad, quad1, sum1, sum2
long buffer

Pri Fusion(gyroX1, gyroY1, gyroZ1, accX1, accY1, accZ1,  pitch1, roll1, bearing1) | b, accyOld, accZold ' these are all pointers and must be addressed as such
  b := true
  buffer := 0
   dira[5] := %1
   accYold := 0
   accZold := 0
   k[0].Start
   k[1].Start
  repeat
      
    if accYold <> Long[accY1] or accZold <>  Long[accZ1]  
       'buffer := accYold
       Long[pitch1] := GetPitch(Long[accY1], Long[gyroX1], @accYold)
       Long[roll1] :=  GetRoll(Long[accZ1], Long[gyroY1], Long[accY1] )
       Long[bearing1] += Long[gyroX1]
       accYold := Long[accY1]
       accZold := Long[accZ1]
       buffer := Long[gyroX1] 
        
     'outa[5] := %1
     {{ b := !b
      if b 
        outa[5] := %1
      else
       outa[5] := %0 }}
PUB Start : ok
  Stop
  'mathlib.Start
  floatlib.Start
  quad := 0
  quad1 := 0
  ok:= cog := cognew(Fusion(@gyroX, @gyroY, @gyroZ, @accX, @accY, @accZ,  @pitch, @roll, @bearing), @stack) + 1
PUB Stop
  'mathlib.Stop
  floatlib.Stop
  if(cog)
    cogstop(cog - 1)
    
Pub Update(pgyroX, pgyroY, pgyroZ, paccX, paccY, paccZ)    'this function gets the IMU data from the main thread
  accX := paccX
  accY := paccY
  accZ := paccZ
  gyroX := pgyroX
  gyroY := pgyroY
  gyroZ := pgyroZ
 
Pub GetData(_pitch, _roll, _bearing) ' this function outputs the pitch, roll, and bearing data to the main thread
  Long[_pitch] := k[0].get_angle'pitch
  Long[_roll] := k[1].get_angle'roll
  Long[_bearing] := bearing  
  
PRI GetPitch(accY2, gyroZ2, accYold) : theta | dg, g, r 'this function calculates the gryo rate, the accelerometer angle and updates the kalman filter for the pitch
  
  g := floatlib.Degrees(floatlib.asin(floatlib.fdiv(floatlib.ffloat(accY2), float(GRAV))))
  'dg := floatlib.fsub(g, floatlib.Degrees(floatlib.asin(floatlib.fdiv(floatlib.ffloat(accYold), float(GRAV)))))  
  r := floatlib.fdiv(floatlib.ffloat(gyroZ2), 16.384)
  k[0].state_update(r)
  k[0].kalman_update(g)
  Long[accyold] := accY2
  theta := floatlib.fRound(g)
  
  
  
   
   
PRI GetRoll(accX2, gyroY2, accOld) : theta | y, g,r            'this function calculates the gryo rate, the accelerometer angle and updates the kalman filter for the pitch 
  g := floatlib.Degrees(floatlib.asin(floatlib.fdiv(floatlib.ffloat(accX2), float(GRAV))))
  r := floatlib.fdiv(floatlib.ffloat(gyroY2), 16.384)
  k[1].state_update(r)
  k[1].kalman_update(g)
  theta := floatlib.fround(g) 
Pri addsum1 : s |X
  s := 0
  repeat X from 0 to 9
    s+=sumX[X] 
Pri addsum2 : s |X
  s := 0
  repeat X from 0 to 9
    s+=sumY[X] 

Pub readBuffer: x 
  x:= buffer